#include <memory>
#include <vector>
#include <string>
#include <cmath>
#include <fstream>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h> // 修复tf2未声明错误
#include <ament_index_cpp/get_package_share_directory.hpp>

// 全局变量用于滑动条 - 修改为RGB参数
int red_min = 0, red_max = 236;
int green_min = 81, green_max = 255;
int blue_min = 0, blue_max = 48;

class RedObjectPoseNode : public rclcpp::Node
{
public:
    RedObjectPoseNode()
        : Node("red_object_pose_node")
    {
        // 从ost.yaml文件读取相机内参
        if (!loadCameraParameters()) {
            RCLCPP_ERROR(this->get_logger(), "Failed to load camera parameters from ost.yaml");
            return;
        }
        


        // 创建窗口和滑动条
        cv::namedWindow("RGB Control", cv::WINDOW_AUTOSIZE);
        // cv::namedWindow("Original Image", cv::WINDOW_AUTOSIZE);
        cv::namedWindow("Red Detection", cv::WINDOW_AUTOSIZE);
        cv::namedWindow("Mask", cv::WINDOW_AUTOSIZE);

        // 创建RGB参数调整滑动条
        cv::createTrackbar("Red Min", "RGB Control", &red_min, 255);
        cv::createTrackbar("Red Max", "RGB Control", &red_max, 255);
        cv::createTrackbar("Green Min", "RGB Control", &green_min, 255);
        cv::createTrackbar("Green Max", "RGB Control", &green_max, 255);
        cv::createTrackbar("Blue Min", "RGB Control", &blue_min, 255);
        cv::createTrackbar("Blue Max", "RGB Control", &blue_max, 255);

        // 订阅矫正后的图像
        sub_ = this->create_subscription<sensor_msgs::msg::Image>(
            "/camera/image_rect", 10,
            std::bind(&RedObjectPoseNode::image_callback, this, std::placeholders::_1));

        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    }

private:
    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        }
        catch (cv_bridge::Exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
            return;
        }
        cv::Mat img = cv_ptr->image;
        // cv::imshow("Original Image", img);

        // RGB分割红色区域
        cv::Mat mask;
        cv::inRange(img, cv::Scalar(blue_min, green_min, red_min), cv::Scalar(blue_max, green_max, red_max), mask);

        // 显示mask
        cv::imshow("Mask", mask);

        // 查找轮廓
        std::vector<std::vector<cv::Point>> contours;
        // 查找轮廓
        // 参数解释:
        // mask: 输入的二值图像
        // contours: 输出的轮廓点集
        // cv::RETR_EXTERNAL: 只检测最外层轮廓
        // cv::CHAIN_APPROX_SIMPLE: 压缩水平、垂直和对角线段，只保留端点
        cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

        // 过滤符合长宽比1:1的正方形轮廓
        std::vector<std::vector<cv::Point>> filtered_contours;
        std::vector<cv::RotatedRect> valid_rects;

        for (const auto &contour : contours)
        {
            // 检查轮廓面积，过滤太小的轮廓
            double area = cv::contourArea(contour);
            if (area < 50 || area > 10000)
                continue;

            // 获取最小外接矩形
            cv::RotatedRect rect = cv::minAreaRect(contour);
            float width = rect.size.width;
            float height = rect.size.height;

            // 确保width是短边，height是长边
            if (width > height)
            {
                std::swap(width, height);
            }

            // 计算长宽比
            float aspect_ratio = height / width;

            // 检查长宽比是否接近1:1（正方形，允许一定误差）
            float target_ratio = 1.0f;
            float tolerance = 0.3f; // 容差范围，正方形应该很接近1:1

            if (std::abs(aspect_ratio - target_ratio) <= tolerance)
            {
                std::cout<<"GOOD square aspect_ratio: "<<aspect_ratio<<std::endl;
                filtered_contours.push_back(contour);
                valid_rects.push_back(rect);

                // RCLCPP_INFO(this->get_logger(), "Found valid square: aspect ratio = %.2f, area = %.2f",
                //             aspect_ratio, area);
            }else{
                std::cout<<"NG aspect_ratio: "<<aspect_ratio<<std::endl;
            }
        }

        if (!filtered_contours.empty())
        {
            // 找到面积最大的有效正方形
            auto largest_iter = std::max_element(filtered_contours.begin(), filtered_contours.end(),
                                                 [](const std::vector<cv::Point> &a, const std::vector<cv::Point> &b)
                                                 {
                                                     return cv::contourArea(a) < cv::contourArea(b);
                                                 });
            int largest_index = std::distance(filtered_contours.begin(), largest_iter);

            cv::RotatedRect rect = cv::minAreaRect(*largest_iter);
            cv::Point2f center = rect.center;
            float w_px = std::min(rect.size.width, rect.size.height);
            float l_px = std::max(rect.size.width, rect.size.height);

            // 计算实际长宽比
            float actual_ratio = l_px / w_px;

            // ===== 位置计算部分 =====
            // 已知物体实际尺寸
            double object_actual_width = 22.0;  // mm (正方形边长)
            double object_actual_area = object_actual_width * object_actual_width;  // mm² (实际面积)
            
            // 计算图像中检测到的矩形面积（像素²）
            double detected_area_px = cv::contourArea(*largest_iter);
            
            // 使用面积比计算Z距离（深度）
            // 原理：面积比 = (实际面积/图像面积) = (Z/f)²
            // 因此：Z = f * sqrt(实际面积/图像面积)
            // 其中 f 是焦距，这里使用fx和fy的平均值
            double f_avg = (fx_ + fy_) / 2.0;  // 平均焦距
            double area_ratio = object_actual_area / detected_area_px;
            double Z = f_avg * sqrt(area_ratio);
            
            // 计算X, Y坐标（相机坐标系）
            // 修正坐标系：X向右为正，Y向上为正（与图像坐标系Y轴相反）
            // X = (u - cx) * Z / fx
            // Y = -(v - cy) * Z / fy  （注意Y轴取负号）
            double X = (center.x - cx_) * Z / fx_;
            double Y = (center.y - cy_) * Z / fy_;  // Y轴反向
            
            // 输出位置信息
            RCLCPP_INFO(this->get_logger(), 
                       "Square Object Position - X: %.2f mm, Y: %.2f mm, Z: %.2f mm",
                       X, Y, Z);
            
            // 验证计算的合理性 - 使用面积验证
            double calculated_area_px = object_actual_area / (area_ratio);
            double area_error = std::abs(calculated_area_px - detected_area_px) / detected_area_px * 100.0;
            
            // 输出验证信息和面积信息
            RCLCPP_INFO(this->get_logger(), 
                       "Area Info - Detected: %.2f px², Expected: %.2f px², Error: %.2f%%",
                       detected_area_px, calculated_area_px, area_error);
            
            // 额外输出面积比信息用于调试
            RCLCPP_INFO(this->get_logger(), 
                       "Area Ratio: %.6f, Actual Area: %.2f mm², Avg Focal Length: %.2f",
                       area_ratio, object_actual_area, f_avg);

            // 可视化 - 绘制所有有效的正方形轮廓
            for (size_t i = 0; i < filtered_contours.size(); i++)
            {
                cv::Scalar color = (i == static_cast<size_t>(largest_index)) ? cv::Scalar(0, 255, 0) : cv::Scalar(0, 165, 255); // 最大的用绿色，其他用橙色
                cv::drawContours(img, filtered_contours, i, color, 2);

                // 绘制正方形框
                cv::Point2f vertices[4];
                valid_rects[i].points(vertices);
                for (int j = 0; j < 4; j++)
                {
                    cv::line(img, vertices[j], vertices[(j + 1) % 4], color, 2);
                }
            }

            // 绘制最大正方形的中心点
            cv::circle(img, center, 5, cv::Scalar(255, 0, 0), -1);

            // 显示位置信息在图像上
            std::string pose_info = "Position: X=" + std::to_string(static_cast<int>(X)) + 
                                   "mm, Y=" + std::to_string(static_cast<int>(Y)) + 
                                   "mm, Z=" + std::to_string(static_cast<int>(Z)) + "mm";
            cv::putText(img, pose_info, cv::Point(10, 120), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 255), 2);

            // 显示面积信息
            std::string area_info = "Area: " + std::to_string(static_cast<int>(detected_area_px)) + " px²";
            cv::putText(img, area_info, cv::Point(10, 150), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 255), 2);

            // 显示正方形信息
            std::string rect_info = "Squares found: " + std::to_string(filtered_contours.size());
            cv::putText(img, rect_info, cv::Point(10, 60), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(255, 255, 255), 2);

            std::string ratio_info = "Aspect ratio: " + std::to_string(actual_ratio).substr(0, 4);
            cv::putText(img, ratio_info, cv::Point(10, 90), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(255, 255, 255), 2);

            // 在图像上显示RGB参数信息
            std::string rgb_info = "RGB Range: R[" + std::to_string(red_min) + "-" + std::to_string(red_max) +
                                   "], G[" + std::to_string(green_min) + "-" + std::to_string(green_max) + 
                                   "], B[" + std::to_string(blue_min) + "-" + std::to_string(blue_max) + "]";
            cv::putText(img, rgb_info, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 2);

            cv::imshow("Red Detection", img);
            cv::waitKey(1);

            // 发布tf (仅位置信息，无旋转)
            geometry_msgs::msg::TransformStamped t;
            t.header.stamp = this->now();
            t.header.frame_id = "camera_color_optical_frame"; // 相机坐标系
            t.child_frame_id = "object";

            t.transform.translation.x = X / 1000.0; // 单位: m
            t.transform.translation.y = Y / 1000.0;
            t.transform.translation.z = Z / 1000.0;

            // 无旋转 (单位四元数)
            tf2::Quaternion q;
            q.setRPY(0, 0, 0);  // 无旋转
            t.transform.rotation.x = q.x();
            t.transform.rotation.y = q.y();
            t.transform.rotation.z = q.z();
            t.transform.rotation.w = q.w();

            tf_broadcaster_->sendTransform(t);
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "No valid 1:1 square found.");
            cv::imshow("Red Detection", img);
        }
        cv::waitKey(1);
    }

private:
    bool loadCameraParameters()
    {
        try {
            std::string yaml_file_path;
            
            // 方法1: 尝试从安装的share目录读取
            try {
                std::string package_path = ament_index_cpp::get_package_share_directory("capture_image");
                yaml_file_path = package_path + "/ost.yaml";
                std::ifstream test_file(yaml_file_path);
                if (test_file.good()) {
                    test_file.close();
                    RCLCPP_INFO(this->get_logger(), "Found ost.yaml in package share directory");
                } else {
                    throw std::runtime_error("File not found in share directory");
                }
            } catch (...) {
                // 方法2: 尝试从源码目录读取
                yaml_file_path = "/home/yelume/01-program/05-mbot-marm/marm_ws/src/capture_image/ost.yaml";
                std::ifstream test_file(yaml_file_path);
                if (test_file.good()) {
                    test_file.close();
                    RCLCPP_INFO(this->get_logger(), "Found ost.yaml in source directory");
                } else {
                    RCLCPP_ERROR(this->get_logger(), "Cannot find ost.yaml file in any expected location");
                    return false;
                }
            }
            
            RCLCPP_INFO(this->get_logger(), "Loading camera parameters from: %s", yaml_file_path.c_str());
            
            // 手动解析YAML文件
            std::ifstream file(yaml_file_path);
            if (!file.is_open()) {
                RCLCPP_ERROR(this->get_logger(), "Cannot open camera calibration file: %s", yaml_file_path.c_str());
                return false;
            }

            std::string line;
            bool in_camera_matrix = false;
            std::vector<double> camera_data;
            
            while (std::getline(file, line)) {
                // 移除行首和行尾的空白字符
                line.erase(0, line.find_first_not_of(" \t"));
                line.erase(line.find_last_not_of(" \t") + 1);
                
                if (line.find("camera_matrix:") != std::string::npos) {
                    in_camera_matrix = true;
                    continue;
                }
                
                if (in_camera_matrix && line.find("data:") != std::string::npos) {
                    // 提取data行中的数据
                    size_t bracket_start = line.find('[');
                    if (bracket_start != std::string::npos) {
                        std::string data_line = line.substr(bracket_start + 1);
                        
                        // 继续读取直到找到闭合括号或数据结束
                        std::string full_data = data_line;
                        while (full_data.find(']') == std::string::npos && std::getline(file, line)) {
                            line.erase(0, line.find_first_not_of(" \t"));
                            line.erase(line.find_last_not_of(" \t") + 1);
                            full_data += " " + line;
                        }
                        
                        // 移除闭合括号
                        size_t bracket_end = full_data.find(']');
                        if (bracket_end != std::string::npos) {
                            full_data = full_data.substr(0, bracket_end);
                        }
                        
                        // 解析数字
                        std::stringstream ss(full_data);
                        std::string token;
                        while (std::getline(ss, token, ',')) {
                            token.erase(0, token.find_first_not_of(" \t"));
                            token.erase(token.find_last_not_of(" \t") + 1);
                            if (!token.empty()) {
                                try {
                                    double value = std::stod(token);
                                    camera_data.push_back(value);
                                } catch (const std::exception& e) {
                                    RCLCPP_WARN(this->get_logger(), "Failed to parse token: %s", token.c_str());
                                }
                            }
                        }
                        break;
                    }
                }
                
                // 如果遇到其他顶级键，退出camera_matrix解析
                if (in_camera_matrix && !line.empty() && 
                    (line.find(' ') != 0) && (line.find('\t') != 0) && 
                    line.find("rows:") == std::string::npos && 
                    line.find("cols:") == std::string::npos && 
                    line.find("data:") == std::string::npos) {
                    break;
                }
            }
            
            file.close();
            
            // 验证我们是否获得了正确数量的参数
            if (camera_data.size() != 9) {
                RCLCPP_ERROR(this->get_logger(), "Invalid camera matrix: expected 9 elements, got %zu", camera_data.size());
                return false;
            }
            
            // 提取内参 (相机矩阵是行主序存储)
            fx_ = camera_data[0];  // [0,0] 元素
            fy_ = camera_data[4];  // [1,1] 元素  
            cx_ = camera_data[2];  // [0,2] 元素
            cy_ = camera_data[5];  // [1,2] 元素

            RCLCPP_INFO(this->get_logger(), "Camera parameters loaded successfully:");
            RCLCPP_INFO(this->get_logger(), "fx: %.6f, fy: %.6f", fx_, fy_);
            RCLCPP_INFO(this->get_logger(), "cx: %.6f, cy: %.6f", cx_, cy_);

            return true;
        }
        catch (const std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "Exception while loading camera parameters: %s", e.what());
            return false;
        }
    }

    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
    double fx_, fy_, cx_, cy_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<RedObjectPoseNode>());
    rclcpp::shutdown();
    return 0;
}